import numpy as np
from scipy.ndimage import binary_dilation, rotate
import matplotlib.pyplot as plt

def compute_free_space(grid_map, resolution, front_dist, rear_dist, side_dist, 
                       min_x, max_x, min_y, max_y, num_theta=16):
    """
    Compute a free space map by taking into account the vehicle's shape and orientation.

    Parameters:
    grid_map (np.ndarray): Binary input grid map where 1 represents obstacles and 0 represents free space.
    resolution (float): Grid resolution in meters.
    front_dist (float): Distance from the steering center to the vehicle's front axle in meters.
    rear_dist (float): Distance from the steering center to the vehicle's rear axle in meters.
    side_dist (float): Distance from the steering center to the vehicle's sides in meters.
    min_x, max_x, min_y, max_y (float): Logical coordinate boundaries of the grid map.
    num_theta (int): Number of discrete orientations to consider (default: 16).

    Returns:
    free_space_map (np.ndarray): Updated free space grid map where 1 represents obstacles and 0 represents free space.
    min_x, max_x, min_y, max_y (float): Logical coordinate boundaries of the output map (unchanged from input).
    """
    # Dynamically generate the vehicle shape relative to the steering center
    front_cells = int(np.ceil(front_dist / resolution))
    rear_cells = int(np.ceil(rear_dist / resolution))
    side_cells = int(np.ceil(side_dist / resolution))
    
    # Construct the vehicle's occupancy matrix
    vehicle_shape = np.zeros((rear_cells + front_cells + 1, 2 * side_cells + 1), dtype=bool)
    vehicle_shape[:, :] = 1  # Fill the vehicle's outline

    # Get the dimensions of the grid map
    height, width = grid_map.shape

    # Initialize the free space map, marking all points as free space (0)
    free_space_map = np.zeros((height, width), dtype=bool)

    # Discretize the angles
    angles = np.linspace(0, 360, num_theta, endpoint=False)

    for theta in angles:
        # Rotate the vehicle shape around the steering center
        rotated_shape = rotate(vehicle_shape, angle=theta, reshape=True, order=1)

        # Inflate obstacles based on the rotated vehicle shape
        inflated_map = binary_dilation(grid_map, structure=rotated_shape > 0.5)

        # Mark inflated regions as obstacles
        free_space_map |= inflated_map

    # Convert the map format: 1 represents obstacles, 0 represents free space
    return free_space_map.astype(np.int8)

